import os
import re
import math
import pandas as pd
import numpy as np

# -------------------------- 1. 配置参数（需根据你的场景修改） --------------------------
# 信号换算参数（可根据设备校准，默认值适用于多数室内场景）
TX_POWER = 10  # 路由器发射功率（dBm）
SYSTEM_LOSS = 5  # 系统损耗（dBm）
ATTENUATION_N = 3.0  # 传播衰减系数（室内取2.0~3.0）
# 文件路径（替换为你的实际路径）
POINT_COORDS_PATH = "point_coords.csv"  # 点位坐标文件
WIFI_DATA_DIR = "wifi_data/"  # WiFi原始txt文件所在文件夹
OUTPUT_PATH = "router_coords.csv"  # 输出路由器坐标文件
# -------------------------- 1. 配置参数（需根据你的场景修改） --------------------------
THRESHOLD_DISTANCE = 250  # 距离阈值（米），超过此值的距离将被过滤
CONSTANT_MY = 0.000005

# -------------------------- 2. 核心工具函数 --------------------------
def dbm_to_distance(dbm):
    """
    函数：将dBm信号强度换算为距离（米）
    参数：dbm - 信号强度（如-73）
    返回：距离（米）
    """
    try:
        # 自由空间传播模型公式
        # numerator = TX_POWER - dbm - SYSTEM_LOSS
        # distance = math.pow(10, numerator / (10 * ATTENUATION_N))
        numerator = - dbm 
        distance = math.pow(10, numerator / 10)*CONSTANT_MY
        return round(distance, 2)  # 保留2位小数
    except Exception as e:
        print(f"dBm换算距离失败：{e}")
        return None

def three_point_location(points, distances):
    """
    函数：通过3个点位的坐标和距离，计算路由器坐标（最小二乘解，降低误差）
    参数：
        points - 3个点位的坐标列表，格式：[(x1,y1), (x2,y2), (x3,y3)]
        distances - 3个点位到路由器的距离列表，格式：[d1, d2, d3]
    返回：路由器坐标（X,Y），保留2位小数
    """
    try:
        # 提取3个点位的坐标
        (x1, y1), (x2, y2), (x3, y3) = points
        d1, d2, d3 = distances

        # 构建线性方程组：Ax = b（最小二乘求解）
        # 修复了这里的语法错误：x2² 改为 x2**2，其他类似
        A = np.array([
            [2*(x2 - x1), 2*(y2 - y1)],
            [2*(x3 - x1), 2*(y3 - y1)]
        ])
        b = np.array([
            (x2**2 - x1**2) + (y2**2 - y1**2) + (d1**2 - d2**2),
            (x3**2 - x1**2) + (y3**2 - y1**2) + (d1**2 - d3**2)
        ])

        # 求解线性方程组（最小二乘，避免因测量误差导致无解）
        X, Y = np.linalg.lstsq(A, b, rcond=None)[0]
        return (round(X, 2), round(Y, 2))
    
    except Exception as e:
        print(f"三点定位计算失败：{e}")
        return (None, None)

def read_wifi_data(file_path):
    """
    函数：读取单个WiFi txt文件，提取“SSID+BSSID→信号强度”的映射（BSSID唯一标识路由器）
    返回：字典，格式：{(SSID, BSSID): dbm}
    """
    wifi_dict = {}
    try:
        with open(file_path, "r", encoding="utf-8") as f:
            content = f.read()
            # 用正则匹配每个WiFi的SSID、BSSID、信号强度（适配你的txt格式）
            # 增强正则表达式的容错性
            pattern = r"(\d+)\.\s*SSID:\s*(\S+)\s+?BSSID:\s*(\S+)\s+?信号强度:\s*(-?\d+)\s*dBm"
            matches = re.findall(pattern, content, re.DOTALL | re.IGNORECASE)
            for match in matches:
                ssid = match[1]
                bssid = match[2]
                dbm = int(match[3])
                wifi_dict[(ssid, bssid)] = dbm  # 用(SSID,BSSID)唯一标识路由器
    except Exception as e:
        print(f"读取WiFi数据文件 {file_path} 失败：{e}")
    
    return wifi_dict

# -------------------------- 3. 主流程：计算路由器坐标并存储 --------------------------
def main():
    try:
        # 1. 读取点位坐标（从CSV文件）
        if not os.path.exists(POINT_COORDS_PATH):
            print(f"错误：点位坐标文件 {POINT_COORDS_PATH} 不存在")
            return
            
        point_df = pd.read_csv(POINT_COORDS_PATH)
        if "点位编号" not in point_df.columns or "横坐标X" not in point_df.columns or "纵坐标Y" not in point_df.columns:
            print("错误：点位坐标文件格式不正确，需要包含'点位编号'、'横坐标X'和'纵坐标Y'列")
            return
            
        point_df.set_index("点位编号", inplace=True)  # 以“点位编号”为索引
        print(f"成功读取 {len(point_df)} 个点位的坐标")

        # 2. 读取所有WiFi数据（从txt文件夹）
        if not os.path.exists(WIFI_DATA_DIR):
            print(f"错误：WiFi数据文件夹 {WIFI_DATA_DIR} 不存在")
            return
            
        wifi_all_data = {}  # 格式：{点位编号: {(SSID,BSSID): dbm}}
        for filename in os.listdir(WIFI_DATA_DIR):
            if filename.endswith(".txt") and "point_" in filename:
                # 从文件名提取点位编号（如“point_01_WiFi...txt”→“point_01”）
                try:
                    # 更健壮的点位编号提取方式
                    point_id_match = re.search(r'point_\d+', filename)
                    if point_id_match:
                        point_id = point_id_match.group()
                    else:
                        # 尝试用原方式提取
                        parts = filename.split("_")
                        if len(parts) >= 2:
                            point_id = f"{parts[0]}_{parts[1]}"
                        else:
                            print(f"警告：无法从文件名 {filename} 提取点位编号，跳过该文件")
                            continue
                            
                    # 读取该点位的WiFi数据
                    file_path = os.path.join(WIFI_DATA_DIR, filename)
                    wifi_data = read_wifi_data(file_path)
                    if wifi_data:
                        wifi_all_data[point_id] = wifi_data
                        print(f"点位 {point_id}：读取到 {len(wifi_data)} 个WiFi热点")
                    else:
                        print(f"点位 {point_id}：未读取到有效WiFi数据")
                except Exception as e:
                    print(f"处理文件 {filename} 时出错：{e}")
                    continue

        if not wifi_all_data:
            print("错误：未读取到任何有效WiFi数据")
            return

        # 3. 筛选“至少出现在3个点位”的路由器（需3个距离才能定位）
        router_point_count = {}  # 统计每个路由器出现的点位数量
        for point_id, wifi_dict in wifi_all_data.items():
            for router_key in wifi_dict.keys():
                if router_key not in router_point_count:
                    router_point_count[router_key] = 0
                router_point_count[router_key] += 1
        
        # 筛选出至少出现在3个点位的路由器
        valid_routers = [key for key, count in router_point_count.items() if count >= 3]
        print(f"\n共找到 {len(valid_routers)} 个可定位的路由器（至少出现在3个点位）")

        if not valid_routers:
            print("警告：没有找到至少出现在3个点位的路由器，无法进行定位计算")
            return

        # 4. 对每个有效路由器，计算其坐标
        router_results = []  # 存储结果：[SSID, BSSID, X, Y, 用到的点位列表]
        for router_key in valid_routers:
            ssid, bssid = router_key
            # 收集该路由器在所有点位的“坐标+距离”
            point_distance_list = []
            for point_id, wifi_dict in wifi_all_data.items():
                if router_key in wifi_dict:
                    # 获取该点位的坐标
                    if point_id not in point_df.index:
                        print(f"警告：点位 {point_id} 在坐标文件中不存在，跳过")
                        continue
                    try:
                        x = float(point_df.loc[point_id, "横坐标X"])
                        y = float(point_df.loc[point_id, "纵坐标Y"])
                        
                        # 换算距离
                        dbm = wifi_dict[router_key]
                        distance = dbm_to_distance(dbm)
                        
                        if distance and distance < THRESHOLD_DISTANCE:  
                            point_distance_list.append((point_id, x, y, distance))
                        elif distance and distance >= THRESHOLD_DISTANCE:
                            print(f"警告：路由器 {ssid} 在点位 {point_id} 的距离 {distance} 米过大，已过滤")
                    except Exception as e:
                        print(f"处理路由器 {ssid} 在点位 {point_id} 的数据时出错：{e}")
                        continue
            
            # 取前3个有效点位计算坐标
            if len(point_distance_list) >= 3:
                point_distance_list.sort(key=lambda x: x[3])  # 假设x[3]是distance
                selected_points = point_distance_list[:3]
                # 提取3个点位的坐标和距离
                points = [(p[1], p[2]) for p in selected_points]
                distances = [p[3] for p in selected_points]
                
                # 三点定位计算
                router_x, router_y = three_point_location(points, distances)
                if router_x is not None and router_y is not None:
                    # 记录结果（含用到的点位，便于后续验证）
                    used_points = [p[0] for p in selected_points]
                    if(router_x<0 ):
                        router_x = 0
                    if(router_y<0 ):
                        router_y = 0
                    if(router_x>1200 ):
                        router_x = 1200
                    if(router_y>1200 ):
                        router_y = 1200
                    router_results.append([ssid, bssid, router_x, router_y, ",".join(used_points)])
                    print(f"路由器 {ssid}（{bssid}）：坐标({router_x},{router_y})，用到点位{used_points}")
                else:
                    print(f"路由器 {ssid}（{bssid}）：定位计算失败")
            else:
                print(f"路由器 {ssid}（{bssid}）：有效点位不足3个（仅{len(point_distance_list)}个），无法计算坐标")

        # 5. 保存结果到CSV文件
        if router_results:
            result_df = pd.DataFrame(
                router_results,
                columns=["SSID", "BSSID", "路由器横坐标X", "路由器纵坐标Y", "用到的点位编号"]
            )
            result_df.to_csv(OUTPUT_PATH, index=False, encoding="utf-8-sig")
            print(f"\n成功保存 {len(result_df)} 个路由器的坐标到 {OUTPUT_PATH}")
        else:
            print("\n没有成功计算出任何路由器的坐标")

    except Exception as e:
        print(f"程序运行出错：{e}")

if __name__ == "__main__":
    main()
# -------------------------- 4. 结束 --------------------------